#include "Turret.h"

namespace Team675
{

Turret::Turret()
{
	m_thetaVic = new Victor(THETA_VIC);
	m_phiVic = new Victor(PHI_VIC);
	m_speedVic = new DoubleVictor(SPEED_VIC_1, SPEED_VIC_2);
	
	m_thetaEnc = new Encoder(THETA_ENCODER);
	m_phiEnc = new Encoder(PHI_ENCODER);
	m_speedEnc = new SpeedEncoder(SPEED_ENCODER);
	m_speedEnc->SetPIDSourceParameter(Encoder::kRate);
	
	m_thetaEnc->Start();
	m_phiEnc->Start();
	m_speedEnc->Start();
	
	m_gyro = new Gyro(GYRO);
	
	m_thetaLimit = new DigitalInput(THETA_LIMIT);
	m_phiLimit = new DigitalInput(PHI_LIMIT);
	
	//m_thetaPIDSource = new ThetaPIDSource(*m_gyro, *m_thetaEnc, 180);
	m_thetaPID = new SendablePIDController(THETA_PID, m_thetaEnc, m_thetaVic, .05);
	m_thetaPID->SetInputRange(THETA_MIN, THETA_MAX);
	m_thetaPID->SetOutputRange(-1.0, 1.0);
	m_thetaPID->Disable();
	m_phiPID = new SendablePIDController(PHI_PID, m_phiEnc, m_phiVic, .05);
	m_phiPID->SetInputRange(0.0, PHI_MAX);
	m_phiPID->SetOutputRange(-1.0, 1.0);
	m_phiPID->Disable();
	m_speedPID = new SendablePIDController(SPEED_PID, m_speedEnc, m_speedVic, .05);
	m_speedPID->SetInputRange(0.0, SPEED_MAX);
	m_speedPID->SetOutputRange(-1.0, 1.0);
	m_speedPID->Disable();
	
	m_canZeroPhi = true;
	ZeroPhi();
	
	SmartDashboard::GetInstance()->PutData("Speed PID", m_speedPID);
	SmartDashboard::GetInstance()->PutData("Theta PID", m_thetaPID);
	SmartDashboard::GetInstance()->PutData("Phi PID", m_phiPID);
}

void Turret::ZeroPhi()
{
	if(m_canZeroPhi)
	{
		m_zeroingPhi = true;
		m_phiPID->Disable();
		m_zeroingTimer.Reset();
		m_zeroingTimer.Start();
	}
	else m_zeroingPhi = false;
}

void Turret::ThetaChangeSetpoint(float _change)
{	
	if(!m_thetaLimit->Get())
	{
		if(m_thetaEnc->Get() > 0.0)
		{
			if(_change > 0.0)
			{
//				_change = 0.0;
				m_thetaPID->SetSetpoint(m_thetaEnc->Get());
			}
			else m_thetaPID->SetSetpoint(m_thetaPID->GetSetpoint() + _change);
			
			SmartDashboard::GetInstance()->PutString("Theta Limit", "Upper Limit");
		}
		else
		{
			if(_change < 0.0)
			{
//				_change = 0.0;
				m_thetaPID->SetSetpoint(m_thetaEnc->Get());
			}
			else m_thetaPID->SetSetpoint(m_thetaPID->GetSetpoint() + _change);
			
			SmartDashboard::GetInstance()->PutString("Theta Limit", "Lower Limit");
		}
	}
	else
	{
		m_thetaPID->SetSetpoint(m_thetaPID->GetSetpoint() + _change);
		
		SmartDashboard::GetInstance()->PutString("Theta Limit", "Not Hit");
	}
}

void Turret::ThetaSetError(float _error)
{
	float t_change = m_thetaEnc->Get() + _error;
	
	if(!m_thetaLimit->Get())
	{
		if(m_thetaEnc->Get() > 0.0)
		{
			if(t_change > 0.0)
			{
				t_change = 0.0;
				SmartDashboard::GetInstance()->PutString("Theta Limit", "Turn Right");
			}
			else SmartDashboard::GetInstance()->PutString("Theta Limit", "Upper Limit");
		}
		else
		{
			if(t_change < 0.0)
			{
				t_change = 0.0;
				SmartDashboard::GetInstance()->PutString("Theta Limit", "Turn Left");
			}
			else SmartDashboard::GetInstance()->PutString("Theta Limit", "Lower Limit");
		}
	}
	else
	{
		SmartDashboard::GetInstance()->PutString("Theta Limit", "Not Hit");
	}
	
	m_thetaPID->SetSetpoint(t_change);
}

void Turret::UpdateAutomatic(Target::TargetPosition _target, float _phiSetpoint, float _speedSetpoint)
{
	if(_target == Target::TARGET_NONE) return;
	
	m_thetaPID->Enable();
	m_speedPID->Enable();
	
	float t_thetaSetpointChange;
	
	LaunchInfo t_launchInfo = m_tracker.Update(_target);
	
	t_launchInfo.Print();

	t_thetaSetpointChange = GetThetaEncoderDist(t_launchInfo.HorizAngleError);
	
	ThetaSetError(t_thetaSetpointChange);
	
	m_speedPID->SetSetpoint(_speedSetpoint);

	
	if(m_zeroingPhi)
	{
		m_phiPID->Disable();
		m_phiVic->Set(PHI_ZERO_SPEED);
	}
	else
	{
		m_phiPID->Enable();
		m_phiPID->SetSetpoint(_phiSetpoint);
	}
	
	if(m_phiLimit->Get())
	{
		m_phiEnc->Reset();
		m_zeroingPhi = false;
		m_phiPID->Enable();
		m_phiPID->SetSetpoint(m_phiEnc->Get() + 5);
	}

	printf("speed: %6d       phi: %6d ", (int) m_speedEnc->PIDGet(), (int) m_phiEnc->PIDGet());
	printf("CAMERA: speed: %6d   phi: %6d\n", (int) GetShootingRollerSpeed(t_launchInfo.LaunchSpeed), (int)GetPhiEncoderDist(t_launchInfo.LaunchVerticalAngle));
	SmartDashboard::GetInstance()->PutDouble("Phi angle", (double) GetPhiEncoderDist(t_launchInfo.LaunchVerticalAngle));
	SmartDashboard::GetInstance()->PutDouble("Theta angle", (double) GetThetaEncoderDist(t_launchInfo.HorizAngleError));
	SmartDashboard::GetInstance()->PutDouble("speed", (double) GetShootingRollerSpeed(t_launchInfo.LaunchSpeed));
}

void Turret::UpdateMotorValues(float _thetaMotorValue, float _phiMotorValue, float _speedMotorValue)
{
	if(m_thetaPID->IsEnabled())
	{
		m_thetaPID->Disable();
	}
	if(m_phiPID->IsEnabled())
	{
		m_phiPID->Disable();
	}
	if(m_speedPID->IsEnabled())
	{
		m_speedPID->Disable();
	}
	if(m_phiLimit->Get())
	{
		m_phiEnc->Reset();
	}
	
	m_thetaVic->Set(_thetaMotorValue);
	m_phiVic->Set(_phiMotorValue);
	m_speedVic->Set(_speedMotorValue);
	
	SmartDashboard::GetInstance()->PutDouble("Phi Motor", (double) m_phiVic->Get());
	SmartDashboard::GetInstance()->PutDouble("Theta Motor", (double) m_thetaVic->Get());
	SmartDashboard::GetInstance()->PutDouble("Speed Motor", (double) m_speedVic->Get());
	SmartDashboard::GetInstance()->PutInt("Theta Encoder", m_thetaEnc->Get());
	SmartDashboard::GetInstance()->PutBoolean("Theta Limit", m_thetaLimit->Get());
	SmartDashboard::GetInstance()->PutInt("Phi Encoder", m_phiEnc->Get());
	if(!m_canZeroPhi)
		SmartDashboard::GetInstance()->PutString("Phi Limit", "Not Found");
	else if(m_phiLimit->Get())
		SmartDashboard::GetInstance()->PutString("Phi Limit", "True");
	else
		SmartDashboard::GetInstance()->PutString("Phi Limit", "False");
	SmartDashboard::GetInstance()->PutDouble("Speed Encoder", m_speedEnc->GetCorrectedRate());
}

void Turret::UpdatePIDValues(float _thetaSetpointChange, float _phiSetpoint, float _speedSetpoint)
{
	m_thetaPID->Enable();
	m_speedPID->Enable();
	
	m_speedPID->SetSetpoint(_speedSetpoint);
	ThetaChangeSetpoint(_thetaSetpointChange);
	
	if(m_zeroingPhi)
	{
		m_phiPID->Disable();
		m_phiVic->Set(PHI_ZERO_SPEED);
	}
	else
	{
		m_phiPID->Enable();
		m_phiPID->SetSetpoint(_phiSetpoint);
	}
	
	if(m_phiLimit->Get())
	{
		m_phiEnc->Reset();
		m_zeroingPhi = false;
		m_phiPID->Enable();
		m_phiPID->SetSetpoint(m_phiEnc->Get() + 5);
	}
	DriverStationLCD::GetInstance()->PrintfLine(DriverStationLCD::kUser_Line1,"speed: %d", (int) m_speedEnc->Get());
	printf("speed: %6d       phi: %6d\n", (int) m_speedEnc->PIDGet(), (int) m_phiEnc->PIDGet());
	
	if(DEBUG_MODE)
	{
		SmartDashboard::GetInstance()->PutDouble("Theta Motor", (double) m_thetaVic->Get());
		SmartDashboard::GetInstance()->PutInt("Theta Encoder", m_thetaEnc->Get());
		SmartDashboard::GetInstance()->PutDouble("Phi Motor", (double) m_phiVic->Get());
		SmartDashboard::GetInstance()->PutInt("Phi Encoder", m_phiEnc->Get());
		if(!m_canZeroPhi)
			SmartDashboard::GetInstance()->PutString("Phi Limit", "Not Found");
		else if(m_phiLimit->Get())
			SmartDashboard::GetInstance()->PutString("Phi Limit", "True");
		else
			SmartDashboard::GetInstance()->PutString("Phi Limit", "False");
		SmartDashboard::GetInstance()->PutDouble("Speed Motor", (double) m_speedPID->Get());
		SmartDashboard::GetInstance()->PutDouble("Speed Encoder", m_speedEnc->GetCorrectedRate());
	}
}
	
float Turret::GetConfidence()
{
	return m_confidence;
}

float Turret::GetShootingRollerSpeed(float _exitSpeed)
{
	return _exitSpeed*360*2 / (TURRET_WHEEL_DIAMETER*M_PI);
}
float Turret::GetThetaEncoderDist(float _radians)
{
	return _radians * DEGREES_PER_RADIAN * THETA_PULSES_PER_DEGREE;
}
float Turret::GetPhiEncoderDist(float _radians)
{
	return (PHI_MAX_ANGLE - (_radians * DEGREES_PER_RADIAN)) * PHI_PULSES_PER_DEGREE;
}

}
